#include "rtx.h"
#include "rtx_inc.h"
#include "globals.h"
#include "debug.h"

void swi_trap (VOID)
{
	UINT32 code;

	// Disable interrupts so that this won't get interrupted
	asm( "move.w #0x2700,%sr" );

	// Back-up all of our registers to the stack to ensure that we do not lose any information
	// asm( "move.l %d0, -(%a7)" );
    asm( "move.l %d1, -(%a7)" );
    asm( "move.l %d2, -(%a7)" );
    asm( "move.l %d3, -(%a7)" );
    asm( "move.l %d4, -(%a7)" );
    asm( "move.l %d5, -(%a7)" );
    asm( "move.l %d6, -(%a7)" );
    asm( "move.l %d7, -(%a7)" );
    asm( "move.l %a0, -(%a7)" );
    asm( "move.l %a1, -(%a7)" );
    asm( "move.l %a2, -(%a7)" );
    asm( "move.l %a3, -(%a7)" );
    asm( "move.l %a4, -(%a7)" );
    asm( "move.l %a5, -(%a7)" );
    asm( "move.l %a6, -(%a7)" );

	// Save the stack pointer
	//asm( "move.l %sp, -20(%a6)");

	asm( "move.l %d0, -4(%a6)" );
	switch(code)
	{
		case 0:
			asm( "move.l %d2, -(%a7)" );
			asm( "move.l %d1, -(%a7)" );
			asm( "jsr kernel_send_message" );
			asm( "move.l (%a7)+, %d1" );
			asm( "move.l (%a7)+, %d2" );

			//ret = kernel_send_message(param1, (VOID *)param2);
			break;

		case 1:
			asm( "move.l %d1, -(%a7)" );
			asm( "jsr kernel_receive_message" );
			asm( "move.l (%a7)+, %d1" );

			//ret = (UINT32) kernel_receive_message(param1);
			break;

		case 2:
			asm( "jsr kernel_request_memory_block" );

			//ret = (UINT32) kernel_request_memory_block();
			break;

		case 3:
			asm( "move.l %d1, -(%a7)" );
			asm( "jsr kernel_release_memory_block" );
			asm( "move.l (%a7)+, %d1" );

			//ret = kernel_release_memory_block((VOID *)param1);
			break;

		case 4:
			asm( "jsr kernel_release_processor" );

			//ret = kernel_release_processor();
			break;

		case 5:
			asm( "move.l %d3, -(%a7)" );
			asm( "move.l %d2, -(%a7)" );
			asm( "move.l %d1, -(%a7)" );
			asm( "jsr kernel_delayed_send" );
			asm( "move.l (%a7)+, %d1" );
			asm( "move.l (%a7)+, %d2" );
			asm( "move.l (%a7)+, %d3" );

			//ret = kernel_delayed_send(param1, (VOID *)param2, param3);
			break;

		case 6:
			asm( "move.l %d2, -(%a7)" );
			asm( "move.l %d1, -(%a7)" );
			asm( "jsr kernel_set_process_priority" );
			asm( "move.l (%a7)+, %d1" );
			asm( "move.l (%a7)+, %d2" );

			//ret = kernel_set_process_priority(param1, param2);
			break;

		case 7:
			asm( "move.l %d1, -(%a7)" );
			asm( "jsr kernel_get_process_priority" );
			asm( "move.l (%a7)+, %d1" );

			//ret = kernel_get_process_priority(param1);
			break;

		default:
			break;
	}

	// Restore the stack pointer
	//asm( "move.l temp_stack_pointer , %sp" );

	// Restore our registers
	asm( "move.l (%a7)+, %a6" );
    asm( "move.l (%a7)+, %a5" );
    asm( "move.l (%a7)+, %a4" );
    asm( "move.l (%a7)+, %a3" );
    asm( "move.l (%a7)+, %a2" );
    asm( "move.l (%a7)+, %a1" );
    asm( "move.l (%a7)+, %a0" );
    asm( "move.l (%a7)+, %d7" );
    asm( "move.l (%a7)+, %d6" );
    asm( "move.l (%a7)+, %d5" );
    asm( "move.l (%a7)+, %d4" );
    asm( "move.l (%a7)+, %d3" );
    asm( "move.l (%a7)+, %d2" );
    asm( "move.l (%a7)+, %d1" );
    //asm( "move.l (%a7)+, %d0" );

	//asm( "move.l -20(%a6), %d0" );
	// Unlink the fp
	asm( "unlk %a6" );
	// And now get the hell out of here
	asm( "rte" );
}

int send_message (int process_ID, void* MessageEnvelope)
{
	int ret = 0;

	//Save Variables
	asm( "move.l %d1, -(%a7)" );
	asm( "move.l %d2, -(%a7)" );

	//Parameters
	asm( "move.l 8(%a6), %d1" );
	asm( "move.l 12(%a6), %d2" );

	//API Code
	asm( "move.l #0, %d0" );
	asm( "trap #0" );

	asm( "move.l (%a7)+, %d2" );
	asm( "move.l (%a7)+, %d1" );

	asm( "move.l %d0, -4(%a6)" );

	return ret;
}

void* receive_message (int *sender_ID)
{
	void* ret = 0;

	//Save Variables
	asm( "move.l %d1, -(%a7)" );

	//Paramters
	asm( "move.l 8(%a6), %d1" );

	//API Code
	asm( "move.l #1, %d0" );
	asm( "trap #0" );

	asm( "move.l (%a7)+, %d1" );

	asm( "move.l %d0, -4(%a6)" );
	return ret;
}

void* request_memory_block ()
{
	void* ret = 0;

	//API Code
	asm( "move.l #2, %d0" );
	asm( "trap #0" );

	asm( "move.l %d0, -4(%a6)" );
	return ret;
}

int release_memory_block (void* MemoryBlock)
{
	int ret = 0;

	//Save Variables
	asm( "move.l %d1, -(%a7)" );

	//Parameters
	asm( "move.l 8(%a6), %d1" );

	//API Code
	asm( "move.l #3, %d0" );
	asm( "trap #0" );

	asm( "move.l (%a7)+, %d1" );

	asm( "move.l %d0, -4(%a6)" );
	return ret;
}

int release_processor ()
{
	int ret = 0;

	//API Code
	asm( "move.l #4, %d0" );
	asm( "trap #0" );

	asm( "move.l %d0, -4(%a6)" );
	return ret;
}

int delayed_send (int process_ID, void* MessageEnvelope, int delay)
{
	int ret = 0;

	//Save Variables
	asm( "move.l %d1, -(%a7)" );
	asm( "move.l %d2, -(%a7)" );
	asm( "move.l %d3, -(%a7)" );

	//Parameters
	asm( "move.l 8(%a6), %d1" );
	asm( "move.l 12(%a6), %d2" );
	asm( "move.l 16(%a6), %d3" );

	//API Code
	asm( "move.l #5, %d0" );
	asm( "trap #0" );

	asm( "move.l (%a7)+, %d1" );
	asm( "move.l (%a7)+, %d2" );
	asm( "move.l (%a7)+, %d3" );

	asm( "move.l %d0, -4(%a6)" );
	return ret;
}

int set_process_priority (int process_ID, int priority)
{
	int ret = 0;

	//Save Variables
	asm( "move.l %d1, -(%a7)" );
	asm( "move.l %d2, -(%a7)" );

	//Parameters
	asm( "move.l 8(%a6), %d1" );
	asm( "move.l 12(%a6), %d2" );

	//API Code
	asm( "move.l #6, %d0" );
	asm( "trap #0" );

	asm( "move.l (%a7)+, %d1" );
	asm( "move.l (%a7)+, %d2" );

	asm( "move.l %d0, -4(%a6)" );
	return ret;
}

int get_process_priority (int process_ID)
{
	int ret = 0;

	//Save Variables
	asm( "move.l %d1, -(%a7)" );

	//Parameters
	asm( "move.l 8(%a6), %d1" );

	//API Code
	asm( "move.l #7, %d0" );
	asm( "trap #0" );

	asm( "move.l (%a7)+, %d1" );

	asm( "move.l %d0, -4(%a6)" );
	return ret;
}
